#include "controller/controller_base.h"
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

ControllerBase::ControllerBase():baseFrameId_("base_link"){
    speedPub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    pathLocalPub_ = nh_.advertise<nav_msgs::Path>("/local_path", 10);
    poseLookaheadPub_ = nh_.advertise<geometry_msgs::PoseStamped>("/lookahead_pose", 10);
    poseReplanPub_ = nh_.advertise<geometry_msgs::PoseStamped>("/replan_pose", 10);
    // path_planner_ptr_ = boost::make_shared<JPSPlanner>();
    path_planner_ptr_ = boost::make_shared<AstarPlanner>();
    localOriginPose_.pose.orientation.w = 1.0;
}

ControllerBase::~ControllerBase(){

}

/*************************************************
* 位姿转换
*************************************************/
// 获取TF坐标变换
bool ControllerBase::getTFPose(tf2_ros::Buffer* tf_buffer, geometry_msgs::PoseStamped& global_pose, string father_frame, string child_frame, ros::Time stamp){
    tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
    geometry_msgs::PoseStamped robot_pose;
    tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
    robot_pose.header.frame_id = child_frame;
    robot_pose.header.stamp = ros::Time();
    // get the global pose of the robot
    try
    {
        // use current time if possible (makes sure it's not in the future)
        if (tf_buffer->canTransform(father_frame, child_frame, stamp))
        {
            geometry_msgs::TransformStamped transform = tf_buffer->lookupTransform(father_frame, child_frame, stamp);
            tf2::doTransform(robot_pose, global_pose, transform);
        }
        // use the latest otherwise
        else
        {
            tf_buffer->transform(robot_pose, global_pose, father_frame);
        }
    }
    catch (tf2::LookupException& ex)
    {
        ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
        return false;
    }
    catch (tf2::ConnectivityException& ex)
    {
        ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
        return false;
    }
    catch (tf2::ExtrapolationException& ex)
    {
        ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
        return false;
    }
    // // check global_pose timeout
    // ros::Time current_time = ros::Time::now();
    // if(current_time.toSec() - global_pose.header.stamp.toSec() > __transform_tolerance)
    // {
    //     ROS_WARN_THROTTLE(1.0,
    //                     "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
    //                     current_time.toSec(), global_pose.header.stamp.toSec(), __transform_tolerance);
    //     return false;
    // }

    return true;        
}

// 获取机器人当前位姿
bool ControllerBase::getRobotPose(tf2_ros::Buffer* tf_buffer, geometry_msgs::PoseStamped& global_pose){
    // 尝试3次
    for(int i=0; i<3; i++){
        if(getTFPose(tf_buffer, global_pose, "map", baseFrameId_, ros::Time::now())){
            return true;
        }
        ros::Duration(0.05).sleep();
    }
    return false;
}

// 全局位姿转局部
geometry_msgs::PoseStamped ControllerBase::convertGlobalPoseToLocal(geometry_msgs::PoseStamped global_pose, 
    geometry_msgs::PoseStamped robot_pose){
    geometry_msgs::PoseStamped local_pose;

    // 方式一
    tf2::Transform global_pose_tf;
    tf2::fromMsg(global_pose.pose, global_pose_tf);
    tf2::Transform robot_pose_tf;
    tf2::fromMsg(robot_pose.pose, robot_pose_tf);

    // 计算相对变换（全局变换到局部变换）
    tf2::Transform local_pose_tf = robot_pose_tf.inverse() * global_pose_tf;
    tf2::toMsg(local_pose_tf, local_pose.pose);
    
    // 方式二
    // double yaw = tf2::getYaw(robot_pose.pose.orientation);
    // double delta_x = global_pose.pose.position.x - robot_pose.pose.position.x;
    // double delta_y = global_pose.pose.position.y - robot_pose.pose.position.y;
    // double cos_yaw = std::cos(-yaw);
    // double sin_yaw = std::sin(-yaw);
    // local_pose.pose.position.x = cos_yaw * delta_x - sin_yaw * delta_y;
    // local_pose.pose.position.y = sin_yaw * delta_x + cos_yaw * delta_y;
    // double yaw_diff = utils::math::diffAngle(
    //     msg_convert::toYaw(robot_pose.pose.orientation),
    //     msg_convert::toYaw(global_pose.pose.orientation)
    // );
    // local_pose.pose.orientation = msg_convert::toGeometryQuaternion(yaw_diff);

    return local_pose; 
}

// 局部位姿转全局
geometry_msgs::PoseStamped ControllerBase::convertLocalPoseToGlobal(geometry_msgs::PoseStamped local_pose, 
    geometry_msgs::PoseStamped robot_pose){
    geometry_msgs::PoseStamped global_pose;

    // 方式一
    tf2::Transform local_pose_tf;
    tf2::fromMsg(local_pose.pose, local_pose_tf);
    tf2::Transform robot_pose_tf;
    tf2::fromMsg(robot_pose.pose, robot_pose_tf);

    // 计算全局位姿（局部位姿在机器人坐标系中的全局变换）
    tf2::Transform global_pose_tf = robot_pose_tf * local_pose_tf;
    tf2::toMsg(global_pose_tf, global_pose.pose);

    // 方式二
    // double yaw = tf2::getYaw(robot_pose.pose.orientation);
    // double delta_x = local_pose.pose.position.x;
    // double delta_y = local_pose.pose.position.y;
    // double cos_yaw = std::cos(yaw);
    // double sin_yaw = std::sin(yaw);
    // global_pose.pose.position.x = robot_pose.pose.position.x + cos_yaw * delta_x - sin_yaw * delta_y;
    // global_pose.pose.position.y = robot_pose.pose.position.y + sin_yaw * delta_x + cos_yaw * delta_y;
    // double yaw_add = utils::math::addAngle(
    //     msg_convert::toYaw(robot_pose.pose.orientation),
    //     msg_convert::toYaw(local_pose.pose.orientation)
    // );
    // global_pose.pose.orientation = msg_convert::toGeometryQuaternion(yaw_add);

    return global_pose;
}

// 全局路径转局部
vector<geometry_msgs::PoseStamped> ControllerBase::convertGlobalPathToLocal(vector<geometry_msgs::PoseStamped> global_path, 
    geometry_msgs::PoseStamped robot_pose){
    vector<geometry_msgs::PoseStamped> local_path;
    for(auto pose : global_path){
        local_path.push_back(convertGlobalPoseToLocal(pose, robot_pose));
    }
    return local_path;
}

// 局部路径转全局
vector<geometry_msgs::PoseStamped> ControllerBase::convertLocalPathToGlobal(vector<geometry_msgs::PoseStamped> local_path, 
    geometry_msgs::PoseStamped robot_pose){
    vector<geometry_msgs::PoseStamped> global_path;
    for(auto pose : local_path){
        global_path.push_back(convertLocalPoseToGlobal(pose, robot_pose));
    }
    return global_path;
}

/*************************************************
* 辅助函数
*************************************************/

// 旋转至目标角度
void ControllerBase::rotateToTGoal(geometry_msgs::PoseStamped robot_pose,
    geometry_msgs::PoseStamped goal_pose, geometry_msgs::Twist& cmd_vel){
    cmd_vel.linear.x = 0;
    cmd_vel.linear.y = 0;

    double angle_diff = utils::math::diffAngle(
        msg_convert::toYaw(robot_pose.pose.orientation),
        msg_convert::toYaw(goal_pose.pose.orientation)
    );
    // 最大减速 v^2 = v0^2 + 2as
    double max_speed_to_stop = maxSpeedToStop(baseConfig_.max_vel_yaw, angle_diff);
    max_speed_to_stop = angle_diff > 0 ? max_speed_to_stop : -max_speed_to_stop;

    // 最大加速限制 [__min_vel_limit ~ __max_vel_limit]
    cmd_vel.angular.z = min(baseConfig_.max_vel_yaw, max(baseConfig_.min_vel_yaw, max_speed_to_stop));
}

// 计算最大减速距离
double ControllerBase::maxSpeedToStop(double vel, double acc){
    // 最大减速 v^2 = v0^2 + 2as
    return sqrt(2 * vel * fabs(acc));
}

// 计算直线刹车距离 v^2 - 0 = 2as
double ControllerBase::maxStopDistance(geometry_msgs::Twist cmd_vel, double acc){
    return std::pow(cmd_vel.linear.x, 2) / (2 * acc);
}

// 检测目标点是否到达  距离判断
bool ControllerBase::isPositionReached(geometry_msgs::PoseStamped robot_pose, 
    geometry_msgs::PoseStamped goal_pose){
    double dist = utils::math::distanceBetweenPoses(goal_pose.pose, robot_pose.pose); 
    LOG(INFO) << "[isPositionReached] dist: " << dist << ", goal_distance_tolerance: " << baseConfig_.goal_distance_tolerance;
    if(fabs(dist) < baseConfig_.goal_distance_tolerance){
        return true;
    }
    return false;       
}


// 检测目标点是否到达  角度判断
bool ControllerBase::isAngleReached(geometry_msgs::PoseStamped robot_pose, 
    geometry_msgs::PoseStamped goal_pose){
    double theta = utils::math::diffAngle(
        msg_convert::toYaw(robot_pose.pose.orientation),
        msg_convert::toYaw(goal_pose.pose.orientation)
    );

    LOG(INFO) << "[isAngleReached] theta: " << theta << ", goal_angle_tolerance: " << baseConfig_.goal_angle_tolerance;
    if(fabs(theta) < baseConfig_.goal_angle_tolerance){
        return true;
    }
    return false;
}

// 检测目标点是否到达
bool ControllerBase::isGoalReached(geometry_msgs::PoseStamped robot_pose, geometry_msgs::PoseStamped goal_pose){
    return isPositionReached(robot_pose, goal_pose) && isAngleReached(robot_pose, goal_pose);
}


// 路径裁剪
void ControllerBase::pruningLocalPath(vector<geometry_msgs::PoseStamped>& global_path, 
    vector<geometry_msgs::PoseStamped>& local_path, double limit_x, double limit_y){

    // 确保路径非空
    if(local_path.empty() || global_path.size() != local_path.size()) {
        LOG(WARNING) << "[pruningLocalPath] local_path is empty or global_path.size != local_path.size";
        // return;
        local_path = global_path;   //更新路径信息
    }

    // 找到最近点
    size_t closest_index = -1;
    for (size_t i = 0; i < local_path.size(); ++i) {
        double dist = utils::math::distanceBetweenPoses(local_path[i].pose, localOriginPose_.pose);
        if (dist < 1.0) {
            closest_index = i;
            break;
        }
    } 

    if(closest_index >= 0 && closest_index < local_path.size()){
        for (size_t i = 0; i < closest_index; ++i) {
            local_path.erase(local_path.begin() + i);
            global_path.erase(global_path.begin() + i);
        }
    }

    // // 如果找到前进方向最近点，则裁剪路径
    // if(closest_index >= 0){
    //     for (size_t i = closest_index; i < local_path.size(); ++i) {
    //         const auto& path_point = local_path[i];
    //         double dx = path_point.pose.position.x - localOriginPose_.pose.position.x;
    //         double dy = path_point.pose.position.y - localOriginPose_.pose.position.y;
    //         if ((std::fabs(dx) > limit_x) || (std::fabs(dy) > limit_y)) {
    //             break;
    //         }
    //         pruned_path.push_back(local_path[i]);
    //     }
    // } 
    
    return;
}

// 发布局部路径
void ControllerBase::publishLocalPath(vector<geometry_msgs::PoseStamped> local_path){
    nav_msgs::Path path;
    path.header.stamp = ros::Time::now();
    path.header.frame_id = "map";
    for(auto pose : local_path){
        geometry_msgs::PoseStamped p;
        p.header = path.header;
        p.pose = pose.pose;
        path.poses.push_back(p);
    }
    pathLocalPub_.publish(path);
}

// 获取前瞻距离
double ControllerBase::getLookAheadDistance(double vt){
  double lookahead_dist = fabs(vt) * baseConfig_.lookahead_time;
  return max(baseConfig_.min_lookahead_distance, 
    min(baseConfig_.max_lookahead_distance, lookahead_dist));
}

// 获取前瞻目标点
bool ControllerBase::getLookaheadGoal(vector<geometry_msgs::PoseStamped> local_path, 
    double lookahead_distance, geometry_msgs::PoseStamped& lookahead_goal, int& index){
    if(local_path.empty()){
        return false;
    }
    // //简单的距离计算，后期优化
    // geometry_msgs::PoseStamped origin_data;
    // for(size_t i = local_path.size()-1; i >=0 ; --i){
    //     double dis = utils::math::distanceBetweenPoses(local_path[i].pose, origin_data.pose);
    //     if(dis < lookahead_distance){
    //         lookahead_goal = local_path[i];
    //         return true;
    //     }
    // }

    // 找到后侧最近点
    size_t closest_index = -1;
    for (size_t i = 0; i < local_path.size(); ++i) {
        double dist = utils::math::distanceBetweenPoses(local_path[i].pose, robot_posestamp_.pose);
        if (dist < lookahead_distance) {
            closest_index = i;
            break;
        }
    } 

    // 沿着索引前进，找到前侧最近点
    if(closest_index >=0){
        for (size_t i = closest_index; i < local_path.size(); ++i) {
            const auto& path_point = local_path[i];
            double dist = utils::math::distanceBetweenPoses(path_point.pose, robot_posestamp_.pose);
            if(dist > lookahead_distance || i == local_path.size()-1) {
                lookahead_goal = local_path[i];
                index = i;
                return true;
            }
        }
    }

    
    // geometry_msgs::PoseStamped origin_data;
    // // Find the first pose which is at a distance greater than the lookahead distance
    // auto goal_pose_it = std::find_if(local_path.begin(), local_path.end(), [&](const geometry_msgs::PoseStamped& ps) {
    //     return utils::math::distanceBetweenPoses(ps.pose, localOriginPose_.pose) >= lookahead_distance;
    // });

    // if(goal_pose_it == local_path.end()){
    //     goal_pose_it = std::prev(local_path.end());
    //     lookahead_goal = *goal_pose_it;
    //     return true;
    // }
    // else{
    //     geometry_msgs::PoseStamped goal_posestamp = *goal_pose_it;
    //     geometry_msgs::PoseStamped prev_posestamp;
    //     if (goal_pose_it == local_path.begin()){
    //         prev_posestamp = localOriginPose_;
    //     }
    //     else{
    //         auto prev_pose_it = std::prev(goal_pose_it);
    //         auto prev_posestamp = *prev_pose_it;
    //     }

    //     // 计算插值位姿  圆心为原点，半径为lookahead_distance
    //     std::pair<double, double> center_p(localOriginPose_.pose.position.x, localOriginPose_.pose.position.y);
    //     std::pair<double, double> prev_p(prev_posestamp.pose.position.x - localOriginPose_.pose.position.x, 
    //         prev_posestamp.pose.position.y - localOriginPose_.pose.position.y);
    //     std::pair<double, double> goal_p(goal_posestamp.pose.position.x - localOriginPose_.pose.position.x,
    //         goal_posestamp.pose.position.y - localOriginPose_.pose.position.y);
    //     vector<pair<double, double>> intersectionPoints = utils::graphis::circleLineIntersection(
    //         prev_p, goal_p, center_p, lookahead_distance
    //     );

    //     if(!intersectionPoints.empty()){
    //         lookahead_goal.pose.position.x = intersectionPoints[0].first;
    //         lookahead_goal.pose.position.y = intersectionPoints[0].second;
    //         double angle = utils::math::angleBetweenPoses(prev_posestamp.pose, goal_posestamp.pose);
    //         lookahead_goal.pose.orientation = msg_convert::toGeometryQuaternion(0.0, 0.0, angle);
    //         return true;
    //     }
    // }
    return false;        
}

// 发布前瞻目标点
void ControllerBase::publishLookaheadGoal(geometry_msgs::PoseStamped lookahead_goal){
    lookahead_goal.header.stamp = ros::Time::now();
    lookahead_goal.header.frame_id = "map";
    poseLookaheadPub_.publish(lookahead_goal);
}

// 获取距离最近的点
bool ControllerBase::getClosedPose(vector<geometry_msgs::PoseStamped> path, int& closest_index){
    // 设定为最大值
    double dis_sqrt = std::numeric_limits<float>::infinity();
    closest_index = -1;
    for (size_t i = 0; i < path.size(); ++i) {
        // double dis = path[i].pose.position.y * path[i].pose.position.y + path[i].pose.position.x * path[i].pose.position.x;
        double dis = squarePose(path[i], robot_posestamp_);
        if (dis < dis_sqrt) {
            dis_sqrt = dis;
            closest_index = i;
        }
    }

    return closest_index != -1;
}

// 计算距离平方
double ControllerBase::squarePose(geometry_msgs::PoseStamped a, geometry_msgs::PoseStamped b){
    double dx = a.pose.position.x - b.pose.position.x;
    double dy = a.pose.position.y - b.pose.position.y;
    return dx * dx + dy * dy;
}

// // 更新代价地图
// void ControllerBase::updateCostmapData(){
//     costmapLocalPtr_->getOccupancyGrid(local_map_);
// }

// bool ControllerBase::valueIsValid(unsigned char mx, unsigned char my){
//     auto costmap_ptr = costmapLocalPtr_->getCostmap2D();
//     unsigned int index = costmap_ptr->getIndex(mx, my);
//     if(index < 0 || index > local_map_.data.size()){
//         return false;
//     }
//     return local_map_.data[index] == 0;
// }

// 校验当前点到目标点之间是否存在障碍物
bool ControllerBase::isPathCollision(vector<geometry_msgs::PoseStamped> local_path){
    // TODO: 通过costmap判断路径上是否存在障碍物
    int closest_index = -1;
    // 计算最近的点
    if(getClosedPose(local_path, closest_index)){
        // 向前搜索avoidance_distance距离，是否存在障碍物
        double avoidance_distance_sqr = baseConfig_.avoidance_distance * baseConfig_.avoidance_distance;
        // 遍历点集，查看是否存在障碍物
        // auto costmap_ptr = costmapLocalPtr_->getCostmap2D();
        
        for(size_t i = 0; i < local_path.size(); ++i) {
            double dis = squarePose(local_path[i], robot_posestamp_);
            if( dis > avoidance_distance_sqr){
                break;
            }
            else{
                unsigned int mx, my;
                map_base_.worldToMap(local_path[i].pose.position.x, local_path[i].pose.position.y, mx, my);
                // cout << "mx: " << mx << ", my: " << my << endl;
                if(!map_base_.valueIsValid(mx, my)){
                    // unsigned char cost = costmap_ptr->getCost(mx, my);
                    // LOG(INFO) << "i: " << i << ", x: " << local_path[i].pose.position.x 
                    //     << ", y: " << local_path[i].pose.position.y
                    //     << ", dis: " << dis
                    //     << ", mx: " << mx << ", my: " << my  << ", cost: " << int(cost);
                    return true;
                }
            }
        }
    }

    return false;
}


// 重新生成局部路径
bool ControllerBase::replanLocalPath(vector<geometry_msgs::PoseStamped>& global_path, 
    vector<geometry_msgs::PoseStamped>& local_path, geometry_msgs::PoseStamped robot_pose){
    int closest_index = -1;
    // 计算最近的点
    if(getClosedPose(local_path, closest_index)){
        // auto costmap_ptr = costmapLocalPtr_->getCostmap2D();  
        // 计算碰撞点位姿索引
        std::pair<bool, int> collision_pose = {false, -1};
        for(size_t i = 0; i < local_path.size(); ++i) {
            unsigned int mx, my;
            map_base_.worldToMap(local_path[i].pose.position.x, local_path[i].pose.position.y, mx, my);
            // cout << "mx: " << mx << ", my: " << my << endl;
            if(!map_base_.valueIsValid(mx, my)){
                collision_pose = {true, i};
                break;
            }
        }
        // 寻找下个无碰撞位姿索引
        std::pair<bool, int> next_no_collision_pose = {false, -1};
        if(collision_pose.first){
            for(size_t i = collision_pose.second; i < local_path.size(); ++i) {
                unsigned int mx, my;
                map_base_.worldToMap(local_path[i].pose.position.x, local_path[i].pose.position.y, mx, my);
                // cout << "mx: " << mx << ", my: " << my << endl;
                if(map_base_.valueIsValid(mx, my)){
                    next_no_collision_pose = {true, i};
                    break;
                }
            }

            // 再次尝试延展距离, 向后搜索avoidance_distance距离，是否存在障碍物
            double avoidance_distance_sqr = baseConfig_.avoidance_distance * baseConfig_.avoidance_distance;
            if(next_no_collision_pose.first){
                for(size_t i = next_no_collision_pose.second; i < local_path.size(); ++i) {
                    if(squarePose(local_path[i], local_path[next_no_collision_pose.second]) > avoidance_distance_sqr){
                        break;
                    }
                    else{
                        unsigned int mx, my;
                        map_base_.worldToMap(local_path[i].pose.position.x, local_path[i].pose.position.y, mx, my);
                        // cout << "mx: " << mx << ", my: " << my << endl;
                        if(map_base_.valueIsValid(mx, my)){
                            next_no_collision_pose = {true, i};
                        }
                        else{
                            break;
                        }
                    }
                }
            }
        }
        else{
            LOG(WARNING) << "no collision_pose found!";
            return false;
        }

        // 重新生成路径
        LOG(INFO) << "index: " << next_no_collision_pose.second << " / " << local_path.size() << endl;
        if(collision_pose.first && next_no_collision_pose.first){
            // 获取新目标点位姿
            geometry_msgs::PoseStamped new_goal_pose = local_path[next_no_collision_pose.second];
            new_goal_pose.header.stamp = ros::Time::now();
            new_goal_pose.header.frame_id = baseFrameId_;
            poseReplanPub_.publish(new_goal_pose);

            // 获取新路径
            // auto local_map_ptr = map_base_.getOccupancyGridPtr();
            // costmapLocalPtr_->getOccupancyGrid(local_map);
            vector<geometry_msgs::Pose> poses;
            // auto local_map_ptr = boost::make_shared<nav_msgs::OccupancyGrid>(local_map);
            // path_planner_ptr_->setMap(local_map_ptr);
            mc::status status = path_planner_ptr_->genPath(robot_posestamp_.pose, new_goal_pose.pose, poses);
            if(!status.success || poses.empty()){
                LOG(WARNING) << "genPath failed!";
                return false;
            }
            LOG(INFO) << "poses size: " << poses.size() 
                // << ", lookahead_index: " << lookahead_index
                << ", local_path size: " << local_path.size()
                << ", no_collision_pose_index: " << next_no_collision_pose.second
                << ", no_collision_pose: " << msg_convert::toString(new_goal_pose.pose); 
                
            // 获取新路径
            // vector<geometry_msgs::Pose> new_local_path;
            // new_local_path.insert(new_local_path.end(), poses.begin(), poses.end()-1);
            // for(int i=next_no_collision_pose.second; i<local_path.size(); ++i){
            //     cout << "i: " << i << ", " << msg_convert::toString(local_path[i].pose);
            //     new_local_path.emplace_back(local_path[i].pose);
            // }
            vector<geometry_msgs::Pose> new_local_path = poses;

            // 平滑路径
            vector<geometry_msgs::Pose> smooth_poses;
            path_planner_ptr_->pathSmooth(new_local_path, smooth_poses);
            smooth_poses = path_planner_ptr_->genPathDirection(smooth_poses, new_local_path.back());   // 生成路径方向
            cout << "smooth_poses size: " << smooth_poses.size() << endl;

            // 更新局部路径
            local_path.resize(smooth_poses.size());
            for(int i=0; i<smooth_poses.size(); ++i){
                local_path[i].pose = smooth_poses[i];
            }
            // global_path = local_path;

            // 更新全局路径
            // global_path = convertLocalPathToGlobal(local_path, robot_pose);
        }
        else{
            LOG(WARNING) << "no next_no_collision_pose found!";
            return false;
        }
    }
    else{
        LOG(WARNING) << "no closest_pose found!";
        return false;
    }
    return true;
}

/*************************************************
* 速度控制
*************************************************/

// 急停控制
void ControllerBase::e_stop(){
    sendEmptyTwist();
}

// 减速控制
void ControllerBase::slow_down(double controller_frequency){
    // v = v0 - at
    geometry_msgs::Twist cmd_vel;
    cmd_vel.linear.x = max(0.0, currCmdVel_.linear.x - (baseConfig_.linear_acc * controller_frequency));
    cmd_vel.linear.y = max(0.0, currCmdVel_.linear.y - (baseConfig_.linear_acc * controller_frequency));
    cmd_vel.angular.z = max(0.0, currCmdVel_.angular.z - (baseConfig_.angular_acc * controller_frequency));
    sendVelocity(cmd_vel);
}

// 停止控制
void ControllerBase::sendEmptyTwist(){
    geometry_msgs::Twist empty_twist;
    currCmdVel_ = empty_twist;          //当前速度数据缓存
    sendVelocity(empty_twist);
}

// 发送速度
void ControllerBase::sendVelocity(geometry_msgs::Twist& cmd_vel){
    // 需要添加限幅和平滑处理

    // 速度限制
    cmd_vel.linear.x = max(min(baseConfig_.max_vel_x, cmd_vel.linear.x), baseConfig_.min_vel_x);   
    cmd_vel.linear.y = 0.0;
    cmd_vel.angular.z = max(min(baseConfig_.max_vel_yaw, cmd_vel.angular.z), baseConfig_.min_vel_yaw);

    // cout << "[sendVelocity] cmd_vel: " << msg_convert::toString(cmd_vel) << endl;
    currCmdVel_ = cmd_vel;              //当前速度数据缓存
    speedPub_.publish(cmd_vel);
}


void ControllerBase::updateCostmapData(boost::shared_ptr<nav_msgs::OccupancyGrid> costmap_ptr){
    map_base_.setMap(costmap_ptr);
    path_planner_ptr_->setMap(costmap_ptr);
}

void ControllerBase::updateRobotPose(geometry_msgs::PoseStamped robot_pose){
    robot_posestamp_ = robot_pose;
}